Self-driving robot with straight-line and turn control

Introduction

Self-driving cars (autonomous vehicles, AVs) have always been a dream of the future, with more and more companies and individuals looking to revolutionize the space with novel algorithms and hardware. Usually, these systems are complex with dozens of cameras, LiDAR, ultrasound, radar, and GPS guidance sensors to complement a powerful multi-core CPU and GPU. This allows modern AVs to run a complex machine-learning model which detects the environmental traffic and road conditions, and then make decisions based on the data.

We wondered if we could simplify a complex AV system to a basic level, yet still achieve the same goal of autonomous driving. In this project, we aim to build a self-driving car with a Raspberry Pi 4, which is a single-board computer with a quad-core ARM Cortex-A72 CPU. We use a Raspberry Pi camera to capture the road ahead, and process the image using OpenCV to detect the lanes. We then calculate a heading angle from this image, and use the angle to steer the vehicle. The car is controlled by two DC motors, and we use a PID controller to adjust the motor speed and direction to keep the vehicle centered in the lane.

However, real AVs require much more than simple vision, since even the most advanced vision systems can be fooled by shadows, reflections, and other environmental factors. Therefore, we supplement our vision system with an inertial measurement unit (IMU) to detect the vehicle's orientation, and photointerrupter encoders to measure the vehicle's speed. We fuse the data from these sensors to adjust the AV speed and direction, supplementing the vision system and ensuring reliable performance even under suboptimal lighting conditions. This project demonstrates how an embedded system can be used to manage multiple sensors and motors to achieve a complex task like autonomous driving. This project showcases a relatively small implementation of real-time, decision-making robotics.

Design

Hardware

We designed our AV atop a simple robot kit used in a previous class assignment. The kit came with two drive motors, two plastic wheels, an acrylic frame, and a 6V battery box taking 4x AA batteries. Given that the kit was only $15, it did not come preassembled, nor did it have a variable PWM motor controller to adjust the motor's rotational speed. So, we added a 6V PWM DC speed controller which accepts a PWM signal and varies an output voltage signal based on the PWM input. This enabled us to adjust the speed of the vehicle. The Raspberry Pi controlled the duty cycle of this PWM input through two GPIO pins, one per motor. Additionally, we set a base PWM frequency of 50Hz: this was the frequency recommended by the motor controller user guide. Addiitonally, below is a diagram of a PWM signal at various duty cycles. Note that as the duty cycle increases, the time spent at the logical "high" state also increases, which directly correlates to a higher average DC voltage and a higher motor speed.

If we were to use the robot in its current state, we would be able to drive it forwards and backwards, but not be able to maintain a perfectly straightline heading. This is because the robot's wheels are not perfectly aligned, and the motors are not perfectly matched. Therefore, sending the same PWM signal (and therefore, the same voltage) to both motors may cause one motor to spin slightly faster than the other, leading to a progressive drift from midline. To address this, we implemented closed-loop motor control using photointerrupters. A photointerrupter is simply a t-slotted sensor with a photodiode on one side (think of this like a solar panel) and LED on the other side. We glued a slotted disk to the drive axle of each motor, and placed the disk in between the t-slot of the photointerrupter. Below is what our sensor looked like after it was installed onto the robot; the red circle is the photointerrupter, while the blue circle is the slotted disk.

When the disk rotates, the slotted disk periodically blocks and allows light to pass from the LED to the photodiode, registering a signal which we could accept via GPIO. By counting the number of pulses (clicks) over a given time period, we calculated the rotational velocity of the disk, and therefore, measured the robot's speed. We used this speed to adjust the PWM duty cycle of the motors, ensuring a constant movement speed regardless of the actual PWM signal sent to the motors. This is the essence of closed-loop motor control; we removed the direct dependency of terrain and battery voltage on the speed of the motors, since the motors would now always spin at the same number of clicks per second to maintain a consistent speed.

Next, the keen reader may note that although the robot may now drive in a straight line, it is still unable to determine its current direction. Therefore, to determine the robot's heading, we fused data from a gyroscope onboard a 9DOF inertial measurement unit (IMU) sensor with camera input. The reason why we needed to use data from both sensors is because neither is perfect:

However, when both sensors are used, the error is relatively small since each sensor's advantages can make up for the disadvntages in the other. The result is a clean, accurate signal of the current and desired headings. The IMU sensor was connected to the Raspberry Pi via I2C, and the camera was connected via the Pi's camera port using a short ribbon cable. Combining the data from both sensors, and processing the image via our custom image processing pipeline, we calculated a heading angle which was used to steer the AV. After some tinkering, we settled on a 95%-5% data bias, which means that 95% of the heading calculation is based on camera input, while 5% is based on the gyroscope. The IMU sensor was connected to the Raspberry Pi over its dedicated I2C channel, and the camera was connected through the Pi's dedicated camera port. Below is a picture of the location of the camera and the IMU gyroscope on our robot. In the next section, we will describe the functionality of the image processing pipeline, as well as the software PID motor control used to move and steer the AV around the curved and straight-line tracks.

Software

The strength of a lane-following and lane-centering autonomous vehicle lies solely on a precise and reliable control system. Our control system solicited input from multiple sensors -- two photointerrupters, a gyroscope, and a camera -- which integrated into a Proportional-Integral-Derivative (PID) controller. This controller dynamically adjusted the rotation angle and vehicle speed 5 times per second, a rate that updated the robot's path frequently enough to maintain the robot's position within the lanes.

Image Processing Pipeline for Lane Detection

The image processing pipeline involved an image feed from a Raspberry Pi camera mounted on the robot, which was passed into the OpenCV computer vision framework, upon which color filtering and computer vision algorithms were employed to determine the robot's path. This path was then translated into the robot's movement.

The first step of the image processing pipeline was the camera feed from the Raspberry Pi camera. The group utilized a low resolution of 160 by 120 pixels to maintain a higher frame rate. If the frame rate was too low, the robot's path would involve sharper turns to compensate for overdrift from the center of the lane. Admittedly, the group could have avoided such sharp turns by reducing the speed of the robot significantly, but this would detract from the appeal of an effecting AV. Since there was very little noise in each image -- either blue thin lines or a gray background -- the group was able to get by with the low resolution. The Python package picamera was used for the image feed, with the entire image processing pipeline for each image taking around 200 ms to execute, all computed locally on the Raspberry Pi 4.

Each frame was then passed was then converted from the RBG color space to the Hue, Saturation, and Value (HSV) color space. This color space is preferred in computer vision systems due to its ability to separate color (hue) from luminance (saturation and value), which enables a more accurate color-based objection detection regardless of lighting variations. The greatest limitation to effective color-detetion schemes is shadow, which desaturate the true color to a bland gray color. Our algorithm faced this limitation during one such corner of the track, but it was able to turn the robot at an acceptable accuracy, enabled primary by using the HSV color space.

Once the HSV color space was extracted, the pipeline focused on detecting the blue color of lane markings. By setting a lower threshold of [90, 120, 0] and an upper threshold of [90, 120, 0] for hue, saturation, and value, the system isolated the blue areas in the frame through the creation of a binary mask. For pixels detected as blue, the mask would define these pixels as white, while for any other color, they were defined as black. Once the mask was created, it was applied to the to the Canny edge detection algorithm with parameters of 50 as the first threshold and 100 as the second. The first parameter with an argument value of 50 represented the lower boundary of the range for merging the blue gradient values, which determines if a given pixel should be considered for edge linking. The second paramter with an argument value of 100 represented the gradient intensity above which a pixel is considered an edge. Any pixel with a gradient magnitude above 50 and below 100 were considered edges. Once these edges were composed, the were then cropped such that only the lower 50 percent of the screen were considered in the lane detection algorithm. This design decision was due to lane markings at the top 50 percent of the screen being too far ahead of the robot's path to be relevant to its current path, along with extraneous blue marking on the walls of our test environment -- Upson 5th floor -- that we did not want influencing our lane detection algorithm.

The processed image, now with highlighted edges, underwent a line detection procedure using the Hough Transform. This method identifies potential lane markings as line segments within the specified region of interest. The group tuned the Hough Transform's parameters to align with our environment. Particularly, no line was less than 50 pixels long, as all lanes were straight lines -- even the curves, as seen in the video. This adjustment significantly reduced the noise sensitivity of the Hough Transform, allowing for a more accurate prediction of the detected lanes within the image.

With the lines detected using Hough Transform, the final part of this pipeline was in determining the midpoint line, where the position and slope of this line was used to determine the steering angle. In determining this angle, this angle, the group considered three separate cases. First, if two lines were detected, then the midpoint's x-coordinate was determined to be the average of the x-coordinate of the left and right lines. If only one line was detected, then only one lane was in the field of view of the camera, or the lightning was too poor for the camera to pick up the second lane. In this case, we consider that the single detedted line has a start and stop (x, y) coordinate. To produce the x-coordinate, the midpoint of the start and stop x-coordinate was taken, as this case often aligned with a single lane spanning the entire width of the screen, which occurred when the robot was turning. Finally, if no lane was detected, then the previous x-coordinate was used as the value of the current x-coordinate. Since only the bottom half the screen was used, the y-coordinate in all three cases was simply the midpoint of the height -- 60 -- since the image resultion was 160 by 120. To determine the steering angle, the arc-tangent angle between the x-coordinate and y-coordinate was computed, as it measured the angle between a straight vertical line at the center of the frame, and the robot's computed midpoint line, starting from an (x, y) coordinate of (120 / 2, 0) and the computed (x, y) coordinate from above. The coordinate of (120 / 2, 0) represents the current position of the robot -- the center of the screen's width at a y-coordinate of 0 -- and the computed (x, y) midpoint coordinate represents the midpoint line of the robot's current path. The computed angle represents how much the robot must drift in the opposite direction to correct it's orientation such that it positions itself within the middle of the two lanes. If the computed steering angle was greater than 90, the robot would steer right proportional to the magnitude of the angle, and likewise, for a computed steering angle less than 90. This angle was then passed to the PID control system for further tuning.

The final step of the image processing pipeline was displaying the detected lanes and midpoint line for debugging and visulation purposes. The computed midpoint line was drawn in red, while the detected lanes were drawn in green, both of which were displayed on the original frame using OpenCV's line drawing function. We showcase an example of the image processing pipeline in the figure below, where the detected lanes are shown in green, and the midpoint line is shown in red.

PID Motor Control

We now describe the integration and functionality of a PID control system, which utilizes sensor inputs from the photointerrupters, gyroscope, and camera, to manage motor rotation speed and vehicle orientation. The PID controller's role is critical in ensuring that the vehicle adheres to its intended path with minimal error.

PID control is a widely used control feedback mechanism in industrial control systems. It continuously calculates an error value as the difference between a desired setpoint and a measured process variable and applies a correction based on proportional, integral, and derivative terms. The proportional term produces an output that is proportional to the current error value. The larger the error, the greater the response. In this sense, proportional control readily tries to minimize large errors, since those will generate the greatest negative control and quickly bring the system back close to equilibrium. The strength of our proportional response can be adjusted by multiplying the error by a constant known as the proportional gain, Kp. In our autonomous vehicle, the proportional control is used to correct the speed of the motors based on the deviation from the center of the lane, which is detected by both the camera and the gyroscope. A larger error results in a larger proportional correction, leaning the vehicle aggressively towards the lane center. However, while proportional control can reduce the magnitude of the error, it does not guarantee its elimination. The primary issue is the steady-state error, which is a persistent difference between the process variable and the set point when the system reaches equilibrium. In an AV, this manifests as the vehicle consistently driving slightly to one side of the lane, even when the error is small. Trying to increase the proportional gain to eliminate the steady-state error can lead to increased system sensitivity and oscillation. High gain values cause the vehicle to react too aggressively to minor positional errors, so the AV zigzags and bounces around the lane center without actually staying in the center.

To address this, we introduce the integral term, which accumulates the error over time and corrects the system based on the accumulated error. This action allows the controller to react not only to the magnitude of the error but also to its duration: the longer the error persists, the greater this term becomes. Similar to the proportional term, the integral term is multiplied by a constant known as the integral gain, Ki. The integral term is particularly useful for eliminating steady-state errors, as it continually adjusts the control effort and drives the cumulative sum of the error (integral of the error) to zero. In our vehicle, this means that any persistent offset from the lane center, regardless of its cause, is continually adjusted until the vehicle returns to the desired trajectory. Again, however, while integral control is powerful for correcting steady-state bias, it can lead to a new problem known as windup. This occurs when the integral control is too large and accumulates too quickly, causing the system to overshoot the setpoint. Once the error is corrected, the controller again takes a considerable amount of time to unwind the accumulated integral, generating instability and oscillation. This is particularly critical in scenarios where the AV temporarily stops or significantly slows down, as the accumulated error can become disproportionately large compared to the actual driving condition.

To mitigate integral windup, we limit the integral gain Ki to be very small compared to Kp. Additionally, we introduce the derivative term, which is proportional to the rate of change of the error. The derivative term is multiplied by the derivative gain constant, Kd. The derivative term is particularly useful for damping the system's response to sudden changes in the error, thus improving stability. In our AV, this means that if the vehicle starts to veer off course more rapidly (perhaps due to a curve), the derivative control helps apply more immediate corrections to gently bring the vehicle back to its intended path without overshooting. By considering how quickly the error is changing, the derivative component counteracts the accumulated error from the integral component before it causes an overshoot.

In conclusion, while proportional control provides the baseline response necessary for our control system, we still require the additonal terms provided by integral and derivative control. Together, these three terms form the PID controller, which is used to manage the motor speed and direction of the AV. We present our control gain parameters below. Note that most of the gain is provided by the proportional system, while the derivative and integral gains are primarily used to elimiate steady-state error and help the system reach equilibrium more quickly.

The PID controller is implemented in software on the Raspberry Pi, and is run in a separate process using Python multiprocessing from the image processing pipeline. The PID controller reads the current error from the camera and gyroscope, and adjusts the motor speed and direction accordingly. The PID controller is also responsible for maintaining a constant the motor speed based on the photointerrupter click rate; if the clicks are faster than expected, the motor PWM duty cycle is reduced, and vice versa.

Testing

Initial testing of the autonomous navigation system involved the use of frames captured from the onboard camera, which was the backbone of an accurate lane detection algorithm. One of the first tasks was to fine-tune the lower and upper thresholds for the HSV mask, essential for accurate lane detection. The group initially experimented with an RGB filter to isolate lane lines. However, this method proved inadequate under varying lighting conditions, particularly in the presence of shadows which significantly altered the detected colors of the lanes.

As testing progressed, challenges emerged when the robot occasionally failed to detect any lane lines. In response, the team implemented a fallback strategy using a moving average of the ten most recently detected steering angles. The detection of no lanes typically occurred during turns, with the previous implementing simply having the robot steer forward. When this occurred, the robot would drift out of the lanes too much that its camera would no longer have the lanes in its field of view, resulting in the robot become stuck. Additionally, the steering inagle was capped such that the robot could turn no more than 60 degrees left or right, as sudden sharp turns typically corresponded with inaccurate lane readings. Without this cap, the robot would turn out of the lane and the onboard camera would once again not have any lanes in its field of view.

We noticed that depending on the slope of the surface we were testing on, the robot would sometimes stall out and fail to move due to its heavy weight. To remedy this issue, we developed a stop detection algorithm using the onboard photointerrupter sensors. Whenever a full stop was detected (defined as 5 consecutive measurements of 0 clicks per second by the photodiode) the algorithm would momentarily increase the duty cycle of each PWM motor to its maximum of 100 for 0.3 ms. This helps the robot overcome the initial static friction and resume movement. This small adjustment helped maintain consistent mobility in response to an overly sensitive and difficult hardware system -- one where even a misaligned back wheel could cause the robot to drift out of the marked lanes.

In the initial approach to navigating curves, the group attempted to implement a method using circular curves rather than straight-line trajectories, where a polynomial fit would be plotted against the detected points of a given curve. However, identifying which point belonged to which lane proved significantly more challenging for polynomial curves than for straight lines, largely due to their increased sensitivity to outliers. This issue was compounded during turns, where sometimes a lane would span the entire width of an image. Yet the robot could only detect segments of the curved lane due to shadows and diminished camera quality, which caused pixel blurring. Consequently, the Hough Transform algorithm often misinterpreted these fragmented detections as two separate lanes. Additionally, the midpoint line drawn from two polynomial lines was frequently inaccurate -- failing to accurately represent the curved path around 30 percent of the time. This led to the robot overshooting its turn and drifting out of the lane.

Due to these challenges, we opted to simplify the representation of lane curves by treating them as straight lines set at a 45-degree angle to the edges of the track. This modification transformed the track into an octagonal shape, with the segments parallel to the x and y axes being significantly longer than those angled at 45 or 135 degrees to the x-axis. This adjustment greatly enhanced the accuracy of the Hough Transform, enabling correct lane detection approximately 80 percent of the time. Despite this improvement, the accuracy remained relatively low compared to the 96 percent detection rate achieved when the robot moved forward, instead of turning. This discrepancy was primarily due to the increased presence of shadows in curved areas and the lane detection algorithm's frequent need to infer the position of the midpoint lane when only one lane was detectable, often a consequence of limited camera field of view during turns.

Given the tight timeframe of the project, five weeks, we chose to utilize straight lines for curve navigation for the reasons mentioned. If more time were available, they would have dedicated efforts to refining the lane detection algorithm to better accommodate curves, aligning more closely with real-world autonomous vehicle scenarios. Moreover, with a higher budget, the group would have invested in a camera capable of more accurate lane detection in low-light conditions and possibly integrated multiple cameras to enhance detection accuracy.

As previously noted, the accuracy of lane detection for curves was 80 percent. This presented a significant challenge, particularly during maneuvers on curves where precise steering angles were critical; any deviation from the ideal path -- either undershooting or overshooting -- risked driving the robot out of the lane. Furthermore, the challenge was compounded when the robot drifted out of a lane while turning on a curve, as it often resulted in detecting zero or only one lane, compared to the detection of two lanes on a straight path. To address this issue and compensate for situations where no lanes were detected, the group employed a moving average of the previous 10 steering angles. This strategy was implemented when no lanes were detected or when the steering angle during a curve deviated by more than 2.5 standard deviations from the moving average.

Additionally, given the increased complexity of navigating turns, the team developed a curve detection algorithm that activated when the robot attempted to turn more than 30 degrees in either direction for at least 2 seconds. Under these conditions, the robot would reduce its speed to 1 click per second, enhancing the camera's ability to accurately detect lanes on curves. This adjustment was crucial because the camera often experienced pixel blurring during turns, leading to diminished image quality. Conversely, when the robot was determined to be moving in a straight line - defined as maintaining a steering angle of less than 10 degrees in either direction for at least 2 seconds - the robot would increase its speed back to 7 clicks per second. This allowed the robot to move faster, taking advantage of the Pi camera's enhanced lane detection capability under straightforward navigational conditions.

Result

With much tuning and a fresh set of batteries, the robot was able to stay within the lanes consistently. Across a 10 minute trial, while navigating in a straight line, the robot stayed within the lanes 98 percent of the time, and when turning curves, 90 percent of the time. Navigating curves effectively was a particularly difficult task, as the Raspberry Pi camera's quality significantly reduced when turning, resulting in correct lane detecting reducing 50 percent. If given a high-speed camera -- one that detect fast events with large changes in the environment such as vehicle turns -- the lane detection algorithm would likely not see such a drastic reduction in lane detection accuracy. On a general note, autonomous vehicles face the most amount of accidents when turning, as the environment changes significantly, along with signficantly more difficulty in accurately detecting curved lanes. The group is proud to have implemented a relatively accurate autonomous vehicle with a 40 dollar budget.

Groups in previous years have pursued similar concepts in AV navigation within lanes, as demonstrated in the project by An and Chu, available here. However, while An and Chu's pathway was relatively straightforward, featuring only a slight bend, our course involved a complete 360 degree turn. Additionally, their lane detection pipeline processed frames at a rate of 1-2 frames per second, whereas we enhanced our image processing pipeline to handle 4-5 frames per second. This improvement significantly boosted the precision and reliability of our autonomous vehicle navigation. Moreover, our robot was capable of moving at least twice as fast when navigating straight paths and maintained comparable speeds during turns. Furthermore, An and Chu's robot exhibited significant wobble in its path, with one of the wheels touching a lane marker at least 50 percent of the time. In contrast, our refined lane detection algorithm resulted in a wheel touching a lane marker only 2 percent of the time on straight paths and approximately 15 percent of the time on curves.

Parts List

• Raspberry Pi 4 Model B, 2GB RAM - Provided in lab
• Robot kit with 2 drive motors, wheels, encoder disks, acrylic frame, motor controller - Provided in lab
• USB-C power bank - $15
• Raspberry Pi Camera V2- Provided in lab
• 2x photointerrupters - $10
• 9DOF IMU sensor - $15
• Blue painters tape - Provided in lab
• LEDs, Resistors and Wires - Provided in lab

Total: $40

References

Photointerrupter Datasheet
IMU Datasheet
Pi Camera V2 Documentation
Pigpio Library Documentation
RPi GPIO Documentation
RPi GPIO Pinout
OpenCV Documentation
Bootstrap Documentation

          import cv2
          import numpy as np
          import math
          import sys
          import time
          import io
          import picamera
          import RPi.GPIO as GPIO
          from icm20948 import ICM20948
          from utils import Side, Direction, Motor
          import multiprocessing as mp
          
          GPIO.setmode(GPIO.BCM)
          
          # Left motor control pins
          GPIO.setup(5, GPIO.OUT) # AIN1
          GPIO.setup(6, GPIO.OUT) # AIN2
          GPIO.setup(26, GPIO.OUT) # PWMA
          GPIO.setup(23, GPIO.IN, pull_up_down=GPIO.PUD_UP) # clickA
          pwmA = GPIO.PWM(26, 50)
          motorA = Motor(Side.MOTORA, Direction.CW, pwmA, 50)
          
          # Right motor control pins
          GPIO.setup(21, GPIO.OUT) # BIN1
          GPIO.setup(20, GPIO.OUT) # BIN2
          GPIO.setup(16, GPIO.OUT) # PWMB
          GPIO.setup(22, GPIO.IN, pull_up_down=GPIO.PUD_UP) # clickB
          pwmB = GPIO.PWM(16, 50)
          motorB = Motor(Side.MOTORB, Direction.CW, pwmB, 50)
          
          def detect_edges(frame):
              hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
              lower_blue = np.array([90, 120, 0], dtype = "uint8")
              upper_blue = np.array([150, 255, 255], dtype="uint8")
              mask = cv2.inRange(hsv,lower_blue,upper_blue)
              # detect edges
              edges = cv2.Canny(mask, 50, 100)
              return edges
          
          def region_of_interest(edges):
              height, width = edges.shape
              mask = np.zeros_like(edges)
          
              # Calculate height for 70% coverage starting from the bottom
              height_cutoff = int(height * 0.70)  # 30% from the bottom
          
              # Set polygon to cover the bottom 70% of the screen
              polygon = np.array([
                  [(0, height),                         # Bottom left
                   (0, height_cutoff),                  # Top left
                   (width, height_cutoff),              # Top right
                   (width, height)]                     # Bottom right
              ], np.int32)
          
              # Fill the polygon on the mask to define the ROI
              cv2.fillPoly(mask, polygon, 255)
              cropped_edges = cv2.bitwise_and(edges, mask)
              return cropped_edges
          
          def detect_line_segments(cropped_edges):
              rho = 1  
              theta = np.pi / 180  
              min_threshold = 10  
              
              line_segments = cv2.HoughLinesP(cropped_edges, rho, theta, min_threshold, 
                                              np.array([]), minLineLength=5, maxLineGap=150)
          
              return line_segments
          
          def average_slope_intercept(frame, line_segments):
              lane_lines = []
              global set_point
          
              if line_segments is None:
                  set_point.value = 1
                  print("no line segments detected")
                  return lane_lines
              else:
                  set_point.value = 7
                  
              _, width,_ = frame.shape
              left_fit = []
              right_fit = []
          
              boundary = 1/3
              left_region_boundary = width * (1 - boundary)
              right_region_boundary = width * boundary
              
              for line_segment in line_segments:
                  for x1, y1, x2, y2 in line_segment:
                      if x1 == x2:
                          print("skipping vertical lines (slope = infinity")
                          continue
                      
                      fit = np.polyfit((x1, x2), (y1, y2), 1)
                      slope = (y2 - y1) / (x2 - x1)
                      intercept = y1 - (slope * x1)
                      
                      if slope < 0:
                          if x1 < left_region_boundary and x2 < left_region_boundary:
                              left_fit.append((slope, intercept))
                      else:
                          if x1 > right_region_boundary and x2 > right_region_boundary:
                              right_fit.append((slope, intercept))
          
              left_fit_average = np.average(left_fit, axis=0)
              if len(left_fit) > 0:
                  lane_lines.append(make_points(frame, left_fit_average))
          
              right_fit_average = np.average(right_fit, axis=0)
              if len(right_fit) > 0:
                  lane_lines.append(make_points(frame, right_fit_average))
          
              return lane_lines
          
          def make_points(frame, line):
              height,_,_ = frame.shape
              
              slope, intercept = line
              
              y1 = height  # bottom of the frame
              y2 = int(y1 / 2)  # make points from middle of the frame down
              
              if slope == 0:
                  slope = 0.1
                  
              x1 = int((y1 - intercept) / slope)
              x2 = int((y2 - intercept) / slope)
              
              return [[x1, y1, x2, y2]]
          
          def display_lines(frame, lines, line_color=(0, 255, 0), line_width=6):
              line_image = np.zeros_like(frame)
              
              if lines is not None:
                  for line in lines:
                      for x1, y1, x2, y2 in line:
                          cv2.line(line_image, (x1, y1), (x2, y2), line_color, line_width)
                          
              line_image = cv2.addWeighted(frame, 0.8, line_image, 1, 1)
              
              return line_image
          
          def display_heading_line(frame, steering_angle, line_color=(0, 0, 255), line_width=5 ):
              heading_image = np.zeros_like(frame)
              height, width, _ = frame.shape
              
              steering_angle_radian = steering_angle / 180.0 * math.pi
              
              x1 = int(width / 2)
              y1 = height
              x2 = int(x1 - height / 2 / math.tan(steering_angle_radian))
              y2 = int(height / 2)
              
              cv2.line(heading_image, (x1, y1), (x2, y2), line_color, line_width)
              heading_image = cv2.addWeighted(frame, 0.8, heading_image, 1, 1)
              
              return heading_image
          
          def get_steering_angle(frame, lane_lines):
              height, width, _ = frame.shape
              if len(lane_lines) == 2:
                  _, _, left_x2, _ = lane_lines[0][0]
                  _, _, right_x2, _ = lane_lines[1][0]
                  mid = int(width / 2)
                  x_offset = (left_x2 + right_x2) / 2 - mid
              elif len(lane_lines) == 1:
                  x1, _, x2, _ = lane_lines[0][0]
                  x_offset = x2 - x1
              else:
                  x_offset = 0
          
              y_offset = int(height / 2)
              angle_to_mid_radian = math.atan(x_offset / y_offset)
              angle_to_mid_deg = int(angle_to_mid_radian * 180.0 / math.pi)
              steering_angle = angle_to_mid_deg + 90
              return steering_angle
          
          def capture_frames(gyro_angle):
              
              with picamera.PiCamera() as camera:
                  camera.resolution = (160, 120)
                  camera.rotation = 180
                  camera.contrast = 80
                  camera.brightness = 50
                  stream = io.BytesIO()
                  
                  while True:
                      stream.seek(0)
                      camera.capture(stream, format='jpeg')
                      data = np.frombuffer(stream.getvalue(), dtype=np.uint8)
                      frame = cv2.imdecode(data, cv2.IMREAD_COLOR)
                      edges = detect_edges(frame)
                      roi = region_of_interest(edges)
                      line_segments = detect_line_segments(roi)
                      lane_lines = average_slope_intercept(frame, line_segments)
                      lane_lines_image = display_lines(frame, lane_lines)
                      steering_angle = get_steering_angle(frame, lane_lines)
                      heading_image = display_heading_line(lane_lines_image, steering_angle)
                      cv2.imwrite("./view.png", heading_image)
          
                      deviation = steering_angle - 90  
                      gyro_angle.value = int((0.95 * deviation) + (0.05 * gyro_angle.value))
                      
                      
          # PID constants for Motor A
          KP_A = 1.3
          KI_A = 0.02
          KD_A = 0.09
          
          # PID constants for Motor B
          KP_B = 1.3
          KI_B = 0.02
          KD_B = 0.09
          
          # PID constants for angle (shared if needed, separate if necessary)
          KP_ANG = 0.08
          KI_ANG = 0.002
          KD_ANG = 0.08
          
          def count_print(filtered_a: mp.Value, filtered_b: mp.Value, gyro_angle: mp.Value):
              
              def moving_average(new_sample, samples, n=5):
                  samples.append(new_sample)
                  if len(samples) > n:
                      samples.pop(0)
                  return sum(samples) / len(samples)
              
              Aon = Bon = False
              Aclicks = Bclicks = []
              while True:
                  t_end = time.time() + 0.25
                  Aclick = Bclick = 0
                  while time.time() < t_end:
                      if GPIO.input(23) and not Aon:
                          Aclick += 1
                          Aon = True
                      elif not GPIO.input(23) and Aon:
                          Aon = False
                      if GPIO.input(22) and not Bon:
                          Bclick += 1
                          Bon = True
                      elif not GPIO.input(22) and Bon:
                          Bon = False
                  filtered_a.value = int(moving_average(Aclick, Aclicks))
                  filtered_b.value = int(moving_average(Bclick, Bclicks))
          
                  print(f'Actual Motor A: {Aclick} cps')
                  print(f'Actual Motor B: {Bclick} cps')
                  print(f'Deviation angle: {gyro_angle.value} d')
                  
          def read_gyro_angle(gyro_angle: mp.Value):
              imu = ICM20948(i2c_addr=0x69)
              dt = 0.04
              while True:
                  gz = imu.read_accelerometer_gyro_data()[-1]
                  gyro_angle.value += int(2* gz * dt)
                  time.sleep(dt)    
          
          def adjust_motor_speed(filtered_a: mp.Value, filtered_b: mp.Value, gyro_angle: mp.Value, motorA: Motor, motorB: Motor, point):
              sum_errorA = sum_errorB = 0
              sum_errorAng = 0
              last_errorA = last_errorB = 0
              last_errorAng = 0
              dt = 0.1  # Time step for PID computation
              
              angular_setpoint = 0  # Define your angular setpoint here (in degrees)
          
              while True:
                  set_point = point.value
                  errorA = set_point - filtered_a.value
                  errorB = set_point - filtered_b.value      
                  sum_errorA += errorA * dt
                  sum_errorB += errorB * dt
                  d_errorA = (errorA - last_errorA) / dt
                  d_errorB = (errorB - last_errorB) / dt
                  
                  adjustmentA = (KP_A * errorA) + (KI_A * sum_errorA) + (KD_A * d_errorA)
                  adjustmentB = (KP_B * errorB) + (KI_B * sum_errorB) + (KD_B * d_errorB)
          
                  # Angle control
                  angular_error = angular_setpoint - gyro_angle.value
                  sum_errorAng += angular_error * dt
                  d_errorAng = (angular_error - last_errorAng) / dt
                  angular_adjustment = (KP_ANG * angular_error) + (KI_ANG * sum_errorAng) + (KD_ANG * d_errorAng)
          
                  # Apply CPS and angle adjustments
                  new_speedA = motorA.get_speed() + adjustmentA - angular_adjustment
                  new_speedB = motorB.get_speed() + adjustmentB + angular_adjustment
          
                  new_speedA = max(min(new_speedA, 100), 0)
                  new_speedB = max(min(new_speedB, 100), 0)
                  
                  motorA.set_speed(int(new_speedA))
                  motorB.set_speed(int(new_speedB))
                  
                  last_errorA = errorA
                  last_errorB = errorB
                  last_errorAng = angular_error
                  
                  time.sleep(dt)
          
          if __name__ == '__main__':
              filtered_a = mp.Value('i', 0)
              filtered_b = mp.Value('i', 0)
              gyro_angle = mp.Value('i', 0)
              set_point = mp.Value('i', 7)
              try:
                  motorA.cw(0)
                  motorB.cw(0)
                  
                  p1 = mp.Process(target=count_print, args=(filtered_a, filtered_b, gyro_angle))
                  p2 = mp.Process(target=read_gyro_angle, args=(gyro_angle,))
                  p3 = mp.Process(target=capture_frames, args=(gyro_angle,))
                  
                  p1.start()
                  p2.start()
                  p3.start()
                  
                  adjust_motor_speed(filtered_a, filtered_b, gyro_angle, motorA, motorB, set_point)
          
                  p1.join()
                  p2.join()
                  p3.join()
          
              except KeyboardInterrupt:
                  GPIO.cleanup()
                  print("bye")
                  exit()

          import RPi.GPIO as GPIO
          from enum import Enum
          
          class Side(Enum):
              MOTORA = 0
              MOTORB = 1
          
          class Direction(Enum):
              STOP = 0
              CW = 1
              CCW = -1
          
          class Motor:
              def __init__(self, side: Side, direction: Direction, pwm: GPIO.PWM, speed: int):
                  self.side = side
                  self.direction = direction
                  self.pwm = pwm
                  self.speed = speed
                  
                  self.pwm.start(self.speed)
                  self.stop()
          
              def get_speed(self) -> int:
                  return self.speed
          
              def set_speed(self, speed: int) -> None:
                  self.pwm.ChangeDutyCycle(speed)
                  self.speed = speed
                  return
          
              def stop(self) -> None:
                  if self.side == Side.MOTORA:
                      GPIO.output(5, 0)
                      GPIO.output(6, 0)
                  elif self.side == Side.MOTORB:
                      GPIO.output(20, 0)
                      GPIO.output(21, 0)
                  return
          
              def cw(self, speed: int) -> None:
                  if speed < 0:
                      speed = 0
                  elif speed > 100:
                      speed = 100
                  if self.side == Side.MOTORA:
                      GPIO.output(5, 1)
                      GPIO.output(6, 0)
                  elif self.side == Side.MOTORB:
                      GPIO.output(20, 1)
                      GPIO.output(21, 0)
                  self.pwm.ChangeDutyCycle(speed)
                  self.speed = speed
                  return
          
              def ccw(self, speed: int) -> None:
                  if speed < 0:
                      speed = 0
                  elif speed > 100:
                      speed = 100
                  if self.side == Side.MOTORA:
                      GPIO.output(5, 0)
                      GPIO.output(6, 1)
                  elif self.side == Side.MOTORB:
                      GPIO.output(20, 0)
                      GPIO.output(21, 1)
                  self.pwm.ChangeDutyCycle(speed)
                  self.speed = speed
                  return